home *** CD-ROM | disk | FTP | other *** search
/ BBS in a Box 3 / BBS in a box - Trilogy III.iso / Files / Prog / D-G / GemsI / Original / NearestPointOnCurve.c < prev    next >
Encoding:
C/C++ Source or Header  |  1992-06-16  |  12.1 KB  |  484 lines  |  [TEXT/MPS ]
  1. /*
  2. Solving the Nearest Point-on-Curve Problem 
  3. and
  4. A Bezier Curve-Based Root-Finder
  5. by Philip J. Schneider
  6. from "Graphics Gems", Academic Press, 1990
  7. */
  8.  
  9.  /*    point_on_curve.c    */        
  10.                                     
  11. #include <stdio.h>
  12. #include <malloc.h>
  13. #include <math.h>
  14. #include "GraphicsGems.h"
  15.  
  16.  
  17. /*
  18.  *  Forward declarations
  19.  */
  20.         Point2  NearestPointOnCurve();
  21. static    int    FindRoots();
  22. static    Point2    *ConvertToBezierForm();
  23. static    double    ComputeXIntercept();
  24. static    int    ControlPolygonFlatEnough();
  25. static    int    CrossingCount();
  26. static    Point2    Bezier();
  27. static    Vector2    *V2Sub();
  28. static    Vector2    V2ScaleII();
  29.  
  30.         int        MAXDEPTH = 64;    /*  Maximum depth for recursion */
  31. #define    EPSILON    (ldexp(1.0,-MAXDEPTH-1)) /*Flatness control value */
  32. #define    DEGREE    3            /*  Cubic Bezier curve        */
  33. #define    W_DEGREE 5            /*  Degree of eqn to find roots of */
  34.  
  35. #ifdef TESTMODE
  36. /*
  37.  *  main :
  38.  *    Given a cubic Bezier curve (i.e., its control points), and some
  39.  *    arbitrary point in the plane, find the point on the curve
  40.  *    closest to that arbitrary point.
  41.  */
  42. main()
  43. {
  44.    
  45.  static Point2 bezCurve[4] = {    /*  A cubic Bezier curve    */
  46.     { 0.0, 0.0 },
  47.     { 1.0, 2.0 },
  48.     { 3.0, 3.0 },
  49.     { 4.0, 2.0 },
  50.     };
  51.     static Point2 arbPoint = { 3.5, 2.0 }; /*Some arbitrary point*/
  52.     Point2    pointOnCurve;         /*  Nearest point on the curve */
  53.  
  54.     /*  Find the closest point */
  55.     pointOnCurve = NearestPointOnCurve(arbPoint, bezCurve);
  56.     printf("pointOnCurve : (%4.4f, %4.4f)\n", pointOnCurve.x,             pointOnCurve.y);
  57. }
  58. #endif /* TESTMODE */
  59.  
  60.  
  61. /*
  62.  *  NearestPointOnCurve :
  63.  *      Compute the parameter value of the point on a Bezier
  64.  *        curve segment closest to some arbtitrary, user-input point.
  65.  *        Return the point on the curve at that parameter value.
  66.  *
  67.  */
  68. Point2 NearestPointOnCurve(P, V)
  69.     Point2     P;            /* The user-supplied point      */
  70.     Point2     *V;            /* Control points of cubic Bezier */
  71. {
  72.     Point2    *w;            /* Ctl pts for 5th-degree eqn    */
  73.     double     t_candidate[W_DEGREE];    /* Possible roots        */     
  74.     int     n_solutions;        /* Number of roots found    */
  75.     double    t;            /* Parameter value of closest pt*/
  76.  
  77.     /*  Convert problem to 5th-degree Bezier form    */
  78.     w = ConvertToBezierForm(P, V);
  79.  
  80.     /* Find all possible roots of 5th-degree equation */
  81.     n_solutions = FindRoots(w, W_DEGREE, t_candidate, 0);
  82.     free((char *)w);
  83.  
  84.     /* Compare distances of P to all candidates, and to t=0, and t=1 */
  85.     {
  86.         double     dist, new_dist;
  87.         Point2     p;
  88.         Vector2  v;
  89.         int        i;
  90.  
  91.     
  92.     /* Check distance to beginning of curve, where t = 0    */
  93.         dist = V2SquaredLength(V2Sub(&P, &V[0], &v));
  94.             t = 0.0;
  95.  
  96.     /* Find distances for candidate points    */
  97.         for (i = 0; i < n_solutions; i++) {
  98.             p = Bezier(V, DEGREE, t_candidate[i], NULL, NULL);
  99.             new_dist = V2SquaredLength(V2Sub(&P, &p, &v));
  100.             if (new_dist < dist) {
  101.                     dist = new_dist;
  102.                     t = t_candidate[i];
  103.             }
  104.         }
  105.  
  106.     /* Finally, look at distance to end point, where t = 1.0 */
  107.         new_dist = V2SquaredLength(V2Sub(&P, &V[DEGREE], &v));
  108.             if (new_dist < dist) {
  109.                 dist = new_dist;
  110.             t = 1.0;
  111.         }
  112.     }
  113.  
  114.     /*  Return the point on the curve at parameter value t */
  115.     printf("t : %4.12f\n", t);
  116.     return (Bezier(V, DEGREE, t, NULL, NULL));
  117. }
  118.  
  119.  
  120. /*
  121.  *  ConvertToBezierForm :
  122.  *        Given a point and a Bezier curve, generate a 5th-degree
  123.  *        Bezier-format equation whose solution finds the point on the
  124.  *      curve nearest the user-defined point.
  125.  */
  126. static Point2 *ConvertToBezierForm(P, V)
  127.     Point2     P;            /* The point to find t for    */
  128.     Point2     *V;            /* The control points        */
  129. {
  130.     int     i, j, k, m, n, ub, lb;    
  131.     double     t;            /* Value of t for point P    */
  132.     int     row, column;        /* Table indices        */
  133.     Vector2     c[DEGREE+1];        /* V(i)'s - P            */
  134.     Vector2     d[DEGREE];        /* V(i+1) - V(i)        */
  135.     Point2     *w;            /* Ctl pts of 5th-degree curve  */
  136.     double     cdTable[3][4];        /* Dot product of c, d        */
  137.     static double z[3][4] = {    /* Precomputed "z" for cubics    */
  138.     {1.0, 0.6, 0.3, 0.1},
  139.     {0.4, 0.6, 0.6, 0.4},
  140.     {0.1, 0.3, 0.6, 1.0},
  141.     };
  142.  
  143.  
  144.     /*Determine the c's -- these are vectors created by subtracting*/
  145.     /* point P from each of the control points                */
  146.     for (i = 0; i <= DEGREE; i++) {
  147.         V2Sub(&V[i], &P, &c[i]);
  148.     }
  149.     /* Determine the d's -- these are vectors created by subtracting*/
  150.     /* each control point from the next                    */
  151.     for (i = 0; i <= DEGREE - 1; i++) { 
  152.         d[i] = V2ScaleII(V2Sub(&V[i+1], &V[i], &d[i]), 3.0);
  153.     }
  154.  
  155.     /* Create the c,d table -- this is a table of dot products of the */
  156.     /* c's and d's                            */
  157.     for (row = 0; row <= DEGREE - 1; row++) {
  158.         for (column = 0; column <= DEGREE; column++) {
  159.             cdTable[row][column] = V2Dot(&d[row], &c[column]);
  160.         }
  161.     }
  162.  
  163.     /* Now, apply the z's to the dot products, on the skew diagonal*/
  164.     /* Also, set up the x-values, making these "points"        */
  165.     w = (Point2 *)malloc((unsigned)(W_DEGREE+1) * sizeof(Point2));
  166.     for (i = 0; i <= W_DEGREE; i++) {
  167.         w[i].y = 0.0;
  168.         w[i].x = (double)(i) / W_DEGREE;
  169.     }
  170.  
  171.     n = DEGREE;
  172.     m = DEGREE-1;
  173.     for (k = 0; k <= n + m; k++) {
  174.         lb = MAX(0, k - m);
  175.         ub = MIN(k, n);
  176.         for (i = lb; i <= ub; i++) {
  177.             j = k - i;
  178.             w[i+j].y += cdTable[j][i] * z[j][i];
  179.         }
  180.     }
  181.  
  182.     return (w);
  183. }
  184.  
  185.  
  186. /*
  187.  *  FindRoots :
  188.  *    Given a 5th-degree equation in Bernstein-Bezier form, find
  189.  *    all of the roots in the interval [0, 1].  Return the number
  190.  *    of roots found.
  191.  */
  192. static int FindRoots(w, degree, t, depth)
  193.     Point2     *w;            /* The control points        */
  194.     int     degree;        /* The degree of the polynomial    */
  195.     double     *t;            /* RETURN candidate t-values    */
  196.     int     depth;        /* The depth of the recursion    */
  197. {  
  198.     int     i;
  199.     Point2     Left[W_DEGREE+1],    /* New left and right         */
  200.               Right[W_DEGREE+1];    /* control polygons        */
  201.     int     left_count,        /* Solution count from        */
  202.             right_count;        /* children            */
  203.     double     left_t[W_DEGREE+1],    /* Solutions from kids        */
  204.                right_t[W_DEGREE+1];
  205.  
  206.     switch (CrossingCount(w, degree)) {
  207.            case 0 : {    /* No solutions here    */
  208.          return 0;    
  209.          break;
  210.     }
  211.     case 1 : {    /* Unique solution    */
  212.         /* Stop recursion when the tree is deep enough    */
  213.         /* if deep enough, return 1 solution at midpoint     */
  214.         if (depth >= MAXDEPTH) {
  215.             t[0] = (w[0].x + w[W_DEGREE].x) / 2.0;
  216.             return 1;
  217.         }
  218.         if (ControlPolygonFlatEnough(w, degree)) {
  219.             t[0] = ComputeXIntercept(w, degree);
  220.             return 1;
  221.         }
  222.         break;
  223.     }
  224. }
  225.  
  226.     /* Otherwise, solve recursively after    */
  227.     /* subdividing control polygon        */
  228.     Bezier(w, degree, 0.5, Left, Right);
  229.     left_count  = FindRoots(Left,  degree, left_t, depth+1);
  230.     right_count = FindRoots(Right, degree, right_t, depth+1);
  231.  
  232.  
  233.     /* Gather solutions together    */
  234.     for (i = 0; i < left_count; i++) {
  235.         t[i] = left_t[i];
  236.     }
  237.     for (i = 0; i < right_count; i++) {
  238.          t[i+left_count] = right_t[i];
  239.     }
  240.  
  241.     /* Send back total number of solutions    */
  242.     return (left_count+right_count);
  243. }
  244.  
  245.  
  246. /*
  247.  * CrossingCount :
  248.  *    Count the number of times a Bezier control polygon 
  249.  *    crosses the 0-axis. This number is >= the number of roots.
  250.  *
  251.  */
  252. static int CrossingCount(V, degree)
  253.     Point2    *V;            /*  Control pts of Bezier curve    */
  254.     int        degree;            /*  Degreee of Bezier curve     */
  255. {
  256.     int     i;    
  257.     int     n_crossings = 0;    /*  Number of zero-crossings    */
  258.     int        sign, old_sign;        /*  Sign of coefficients    */
  259.  
  260.     sign = old_sign = SGN(V[0].y);
  261.     for (i = 1; i <= degree; i++) {
  262.         sign = SGN(V[i].y);
  263.         if (sign != old_sign) n_crossings++;
  264.         old_sign = sign;
  265.     }
  266.     return n_crossings;
  267. }
  268.  
  269.  
  270.  
  271. /*
  272.  *  ControlPolygonFlatEnough :
  273.  *    Check if the control polygon of a Bezier curve is flat enough
  274.  *    for recursive subdivision to bottom out.
  275.  *
  276.  */
  277. static int ControlPolygonFlatEnough(V, degree)
  278.     Point2    *V;        /* Control points    */
  279.     int     degree;        /* Degree of polynomial    */
  280. {
  281.     int     i;            /* Index variable        */
  282.     double     *distance;        /* Distances from pts to line    */
  283.     double     max_distance_above;    /* maximum of these        */
  284.     double     max_distance_below;
  285.     double     error;            /* Precision of root        */
  286.     Vector2     t;            /* Vector from V[0] to V[degree]*/
  287.     double     intercept_1,
  288.                intercept_2,
  289.                left_intercept,
  290.                right_intercept;
  291.     double     a, b, c;        /* Coefficients of implicit    */
  292.                         /* eqn for line from V[0]-V[deg]*/
  293.  
  294.     /* Find the  perpendicular distance        */
  295.     /* from each interior control point to     */
  296.     /* line connecting V[0] and V[degree]    */
  297.     distance = (double *)malloc((unsigned)(degree + 1) *                     sizeof(double));
  298.     {
  299.     double    abSquared;
  300.  
  301.     /* Derive the implicit equation for line connecting first *'
  302.     /*  and last control points */
  303.     a = V[0].y - V[degree].y;
  304.     b = V[degree].x - V[0].x;
  305.     c = V[0].x * V[degree].y - V[degree].x * V[0].y;
  306.  
  307.     abSquared = (a * a) + (b * b);
  308.  
  309.         for (i = 1; i < degree; i++) {
  310.         /* Compute distance from each of the points to that line    */
  311.             distance[i] = a * V[i].x + b * V[i].y + c;
  312.             if (distance[i] > 0.0) {
  313.                 distance[i] = (distance[i] * distance[i]) / abSquared;
  314.             }
  315.             if (distance[i] < 0.0) {
  316.                 distance[i] = -((distance[i] * distance[i]) /                         abSquared);
  317.             }
  318.         }
  319.     }
  320.  
  321.  
  322.     /* Find the largest distance    */
  323.     max_distance_above = 0.0;
  324.     max_distance_below = 0.0;
  325.     for (i = 1; i < degree; i++) {
  326.         if (distance[i] < 0.0) {
  327.             max_distance_below = MIN(max_distance_below, distance[i]);
  328.         };
  329.         if (distance[i] > 0.0) {
  330.             max_distance_above = MAX(max_distance_above, distance[i]);
  331.         }
  332.     }
  333.     free((char *)distance);
  334.  
  335.     {
  336.     double    det, dInv;
  337.     double    a1, b1, c1, a2, b2, c2;
  338.  
  339.     /*  Implicit equation for zero line */
  340.     a1 = 0.0;
  341.     b1 = 1.0;
  342.     c1 = 0.0;
  343.  
  344.     /*  Implicit equation for "above" line */
  345.     a2 = a;
  346.     b2 = b;
  347.     c2 = c + max_distance_above;
  348.  
  349.     det = a1 * b2 - a2 * b1;
  350.     dInv = 1.0/det;
  351.     
  352.     intercept_1 = (b1 * c2 - b2 * c1) * dInv;
  353.  
  354.     /*  Implicit equation for "below" line */
  355.     a2 = a;
  356.     b2 = b;
  357.     c2 = c + max_distance_below;
  358.     
  359.     det = a1 * b2 - a2 * b1;
  360.     dInv = 1.0/det;
  361.     
  362.     intercept_2 = (b1 * c2 - b2 * c1) * dInv;
  363.     }
  364.  
  365.     /* Compute intercepts of bounding box    */
  366.     left_intercept = MIN(intercept_1, intercept_2);
  367.     right_intercept = MAX(intercept_1, intercept_2);
  368.  
  369.     error = 0.5 * (right_intercept-left_intercept);    
  370.     if (error < EPSILON) {
  371.         return 1;
  372.     }
  373.     else {
  374.         return 0;
  375.     }
  376. }
  377.  
  378.  
  379.  
  380. /*
  381.  *  ComputeXIntercept :
  382.  *    Compute intersection of chord from first control point to last
  383.  *      with 0-axis.
  384.  * 
  385.  */
  386. static double ComputeXIntercept(V, degree)
  387.     Point2     *V;            /*  Control points    */
  388.     int        degree;         /*  Degree of curve    */
  389. {
  390.     double    XLK, YLK, XNM, YNM, XMK, YMK;
  391.     double    det, detInv;
  392.     double    S, T;
  393.     double    X, Y;
  394.  
  395.     XLK = 1.0 - 0.0;
  396.     YLK = 0.0 - 0.0;
  397.     XNM = V[degree].x - V[0].x;
  398.     YNM = V[degree].y - V[0].y;
  399.     XMK = V[0].x - 0.0;
  400.     YMK = V[0].y - 0.0;
  401.  
  402.     det = XNM*YLK - YNM*XLK;
  403.     detInv = 1.0/det;
  404.  
  405.     S = (XNM*YMK - YNM*XMK) * detInv;
  406.     T = (XLK*YMK - YLK*XMK) * detInv;
  407.     
  408.     X = 0.0 + XLK * S;
  409.     Y = 0.0 + YLK * S;
  410.  
  411.     return X;
  412. }
  413.  
  414.  
  415. /*
  416.  *  Bezier : 
  417.  *    Evaluate a Bezier curve at a particular parameter value
  418.  *      Fill in control points for resulting sub-curves if "Left" and
  419.  *    "Right" are non-null.
  420.  * 
  421.  */
  422. static Point2 Bezier(V, degree, t, Left, Right)
  423.     int     degree;        /* Degree of bezier curve    */
  424.     Point2     *V;            /* Control pts            */
  425.     double     t;            /* Parameter value        */
  426.     Point2     *Left;        /* RETURN left half ctl pts    */
  427.     Point2     *Right;        /* RETURN right half ctl pts    */
  428. {
  429.     int     i, j;        /* Index variables    */
  430.     Point2     Vtemp[W_DEGREE+1][W_DEGREE+1];
  431.  
  432.  
  433.     /* Copy control points    */
  434.     for (j =0; j <= degree; j++) {
  435.         Vtemp[0][j] = V[j];
  436.     }
  437.  
  438.     /* Triangle computation    */
  439.     for (i = 1; i <= degree; i++) {    
  440.         for (j =0 ; j <= degree - i; j++) {
  441.             Vtemp[i][j].x =
  442.                   (1.0 - t) * Vtemp[i-1][j].x + t * Vtemp[i-1][j+1].x;
  443.             Vtemp[i][j].y =
  444.                   (1.0 - t) * Vtemp[i-1][j].y + t * Vtemp[i-1][j+1].y;
  445.         }
  446.     }
  447.     
  448.     if (Left != NULL) {
  449.         for (j = 0; j <= degree; j++) {
  450.             Left[j]  = Vtemp[j][0];
  451.         }
  452.     }
  453.     if (Right != NULL) {
  454.         for (j = 0; j <= degree; j++) {
  455.             Right[j] = Vtemp[degree-j][j];
  456.         }
  457.     }
  458.  
  459.     return (Vtemp[degree][0]);
  460. }
  461.  
  462.  
  463. static Vector2 *V2Sub(a, b, c)
  464.     Vector2    *a, *b, *c;
  465. {
  466.     c->x = a->x - b->x;
  467.     c->y = a->y - b->y;
  468.  
  469.     return (c);
  470. }
  471.  
  472.  
  473. static Vector2 V2ScaleII(v, s)
  474.     Vector2    *v;
  475.     double    s;
  476. {
  477.     Vector2 result;
  478.  
  479.     result.x = v->x * s; result.y = v->y * s;
  480.     return (result);
  481. }
  482.  
  483.  
  484.